# ros carla坐标绘制到地图中
import cv2
import numpy as np
import math


def get_RT(map):
    R, T = None, None
    if map == 'Town01':
        # Town01的变换矩阵(ok)
        R = np.array(
            [
                [1.608978933495218, -0.0006133637127350534],
                [0.002967129565265202, -1.616573483032402],
            ]
        )
        T = np.array([[21.96085734252182], [73.98491744019127]])
        err_xy = 5
        loacl_map_h_w = 140
    elif map == 'Town02':
        # Town02的变换矩阵(ok)
        R = np.array(
            [
                [2.853288729947334, -0.001954564367013564],
                [-0.01084418875167121, -2.902583603546258],
            ]
        )
        T = np.array([[75.19143327247443], [-271.2606785755725]])
        err_xy = 1.5
        loacl_map_h_w = 150
    elif map == 'Town03':
        # Town03的变换矩阵(ok very nice)
        R = np.array(
            [
                [1.42539846369879, -0.000613806695560062],
                [-0.006309288645396612, -1.42529645694144],
            ]
        )
        T = np.array([[266.8903605584413], [321.8950052342699]])
        # T = np.array([[265], [320]])
        err_xy = 5
        loacl_map_h_w = 150
    elif map == 'Town04':
        # Town04的变换矩阵(ok)
        R = np.array(
            [
                [0.6945775929658323, -0.00242031540532944],
                [-0.001151290440462326, -0.7332215587816061],
            ]
        )
        T = np.array([[374.2232704087302], [303.5616340310737]])
        err_xy = 5
        loacl_map_h_w = 150
    elif map == 'Town05':
        # Town05的变换矩阵(not ok 高架桥误差大)
        R = np.array(
            [
                [1.151724847113483, 0.04979182850230186],
                [-0.04002015696056613, -1.193283702860338],
            ]
        )
        T = np.array([[387.2353315551624], [325.3770351204567]])
        err_xy = 5
        loacl_map_h_w = 128
    elif map == 'Town06':
        # Town06的变换矩阵（ok）
        R = np.array(
            [
                [0.622355330577449, 0.005832891756840823],
                [-0.002292540060410386, -0.5624124260904821],
            ]
        )
        T = np.array([[242.799258978577], [278.7354311222649]])
        err_xy = 5
        loacl_map_h_w = 128
    elif map == 'Town07':
        # Town07的变换矩阵(not ok 误差大)
        R = np.array(
            [
                [1.641298667149452, -0.01306631310848138],
                [0.03929811782735146, -1.530002461260377],
            ]
        )
        T = np.array([[461.2455550308022], [399.1818917817909]])
        err_xy = 1
        loacl_map_h_w = 128
    return R, T


def transform_carla2img(loc, R, T):
    """carla世界坐标系 转 图像坐标系

    Args::
    Returns:
        img_loc (np shape=(2,1)): 图像坐标系
    """
    carla_loc = loc
    # img=R*carla+T
    img_loc = R.dot(carla_loc) + T
    # img_loc = np.matmul(R, carla_loc) + T
    # img_loc = carla_loc + T
    u = math.floor(max(img_loc[0, 0], 0))
    v = math.floor(max(img_loc[1, 0], 0))
    img_loc = np.array([[u], [v]])
    return img_loc


def draw_map(map, loc, save_path=None):
    pix_x = loc[0][0]
    pix_y = loc[1][0]
    cv2.namedWindow("MAP", 0)
    # cv2.resizeWindow("MAP", 800, 600)
    cv2.circle(map, (pix_x, pix_y), 5, (255, 0, 0), -1)  # BRG 蓝色-起点
    cv2.resizeWindow("MAP", 1920, 1080)
    cv2.imshow("MAP", map)
    cv2.waitKey(1)
    if save_path != None:
        cv2.imwrite(save_path, map)


if __name__ == "__main__":
    # 1-从读取txt读取carla坐标
    town = 'Town03'  # 需要修改
    idx = '02'  # 需要修改
    pose_dir = f'/media/wrc/E/lbw/data/carla/20230427/{town}/poses/{idx}.txt'
    pose_img_dir = pose_dir.split('.')[0] + '.jpg'
    map = cv2.imread(
        f'/home/wrc/study/code/carla_tutorial/carlaMap/CARLAmap/{town}.jpg'
    )
    f = open(pose_dir, mode='r')
    R, T = get_RT(map=town)
    # 记录一下起点
    start_x,start_y=None,None
    lines = list(f.readlines())
    for i in range(len(lines)):
        line = lines[i]
        data = line.split(' ')
        carla_x = float(data[0])
        carla_y = float(data[1])
        carla_loc = np.array([[carla_x], [carla_y]])
        loc_img = transform_carla2img(carla_loc, R, T)
        pix_x = loc_img[0][0]
        pix_y = loc_img[1][0]
        cv2.namedWindow("MAP", 0)
        # cv2.resizeWindow("MAP", 800, 600)
        cv2.resizeWindow("MAP", 1920, 1080)
        if i == 0:
            start_x,start_y=pix_x, pix_y
        cv2.circle(map, (pix_x, pix_y), 5, (0,255,0), -1)
    cv2.circle(map, (start_x, start_y), 5, (255,0,0), -1) # B 蓝色起点
    cv2.circle(map, (pix_x, pix_y), 5, (0,0,255), -1) # B 红色终点
    cv2.imshow("MAP", map)
    cv2.waitKey(0)
    cv2.imwrite(pose_img_dir, map)

    # 2-绘图

    # loc_carla = np.array([[99.63], [41.25]])
    # loc_img = transform_carla2img(loc_carla, R, T)
    # map = cv2.imread('/home/wrc/study/code/carla_tutorial/carlaMap/CARLAmap/Town05.jpg')
    # draw_map(map, loc_img)
